﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace SmartMathLibrary.MinPack
{
    /// <summary>
    /// Uses householder transformations with column pivoting (optional) to compute a qr factorization 
    /// of the m by n matrix a.
    /// </summary>
    [Serializable]
    public static class QrfacClass
    {
        /// <summary>
        /// Uses householder transformations with column pivoting (optional) to compute a qr factorization 
        /// of the m by n matrix a.
        /// </summary>
        /// <param name="m">Input variable set to the number of rows of a.</param>
        /// <param name="n">Input variable set to the number of columns of a.</param>
        /// <param name="a">Contains the matrix for which the qr factorization is to be computed.</param>
        /// <param name="pivot">If pivot is set true, then column pivoting is enforced. if pivot is set false, 
        /// then no column pivoting is done.</param>
        /// <param name="ipvt">Defines the permutation matrix p such that a*p = q*r.</param>
        /// <param name="rdiag">Contains the diagonal elements of r.</param>
        /// <param name="acnorm">Contains the norms of the corresponding columns of the input matrix a.</param>
        /// <param name="wa">A work array of length n.</param>
        public static void Qrfac(int m, int n, double[,] a, int pivot, int[] ipvt,
                                 double[] rdiag, double[] acnorm, double[] wa)
        {
            double[] dpmpar = {
                                  2.220446049250313e-16,
                                  2.225073858507201e-308,
                                  1.797693134862316e-308
                              };

            int i, j, jp1, k, kmax;
            double ajnorm, sum, temp;

            //get machine precision
            double epsmch = dpmpar[0];
            //compute the initial column norms and initialize several arrays
            for (j = 0; j < n; j++)
            {
                acnorm[j] = EnormClass.Colnorm(m, j, 0, a);
                rdiag[j] = acnorm[j];
                wa[j] = rdiag[j];
                if (pivot == 1)
                {
                    ipvt[j] = j;
                }
            }
            //reduce a to r with householder transformations
            int minmn = (m < n) ? m : n;
            for (j = 0; j < minmn; j++)
            {
                if (pivot == 1)
                {
                    //bring column with largest norm into the pivot position
                    kmax = j;
                    for (k = j; k < n; k++)
                    {
                        if (rdiag[k] > rdiag[kmax])
                        {
                            kmax = k;
                        }
                    }
                    if (kmax != j)
                    {
                        for (i = 0; i < m; i++)
                        {
                            temp = a[j, i];
                            a[j, i] = a[kmax, i];
                            a[kmax, i] = temp;
                        }
                        rdiag[kmax] = rdiag[j];
                        wa[kmax] = wa[j];
                        k = ipvt[j];
                        ipvt[j] = ipvt[kmax];
                        ipvt[kmax] = k;
                    }
                }
                //compute the householder transformation
                ajnorm = EnormClass.Colnorm(m, j, j, a);
                if (ajnorm != 0.0)
                {
                    if (a[j, j] < 0.0)
                    {
                        ajnorm = -ajnorm;
                    }
                    for (i = j; i < m; i++)
                    {
                        a[j, i] /= ajnorm;
                    }
                    a[j, j] += 1.0;
                    jp1 = j + 1;
                    if (n > jp1)
                    {
                        for (k = jp1; k < n; k++)
                        {
                            sum = 0.0;
                            for (i = j; i < m; i++)
                            {
                                sum += a[j, i] * a[k, i];
                            }
                            temp = sum / a[j, j];
                            for (i = j; i < m; i++)
                            {
                                a[k, i] -= temp * a[j, i];
                            }
                            if (!(pivot == 1) || !(rdiag[k] == 1))
                            {
                                continue;
                            }
                            temp = a[k, j] / rdiag[k];
                            rdiag[k] *= Math.Sqrt(Math.Max(0.0, 1.0 - temp * temp));
                            if (0.5 * (rdiag[k] * rdiag[k] / (wa[k] * wa[k])) > epsmch)
                            {
                                continue;
                            }
                            rdiag[k] = EnormClass.Colnorm(m, k, jp1, a);
                            wa[k] = rdiag[k];
                        }
                    }
                }
                rdiag[j] = -ajnorm;
            }
        }
    }
}